/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/helpInc/steer_cmd.h
 * @Description  : 定义了SteerCmd类，用于管理AGV的转向命令。 
 *                 提供了初始化、触发命令、打开/关闭功能、设置指令速度、清除命令等功能。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 17:00:48
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once 
#include "rclcpp/rclcpp.hpp"
#include "tools.h"
#include <agv_msgs/msg/do_steer_dir.hpp>
#include <agv_msgs/msg/steer_dir_feed_back.hpp>
#include "timestamp.h"
#include "geometry.h"
// 储存原地打舵是命令数据
class SteerCmd
{
public:
    /**
     * @brief 初始化转向命令对象
     *
     * 初始化转向命令对象，设置初始状态。
     */
    SteerCmd() {
        cmd_id_ = 1;
        msg_cmd_.id = 0;
        msg_cmd_.open = false;
        msg_cmd_.prohibit_limit_flip = false;
        msg_cmd_.steer_dir = 0;
        target_delay_once_.Stop();
        DEBUG_OUT("Init SteerCmd clear");
    }

    /**
     * @brief 触发命令，并返回当前命令ID
     *
     * 如果命令ID超过65534，则重置为0。
     *
     * @return 返回当前的命令ID
     */
    int64_t Trigger() {
        if (++cmd_id_ > 65534) {
            cmd_id_ = 0;
        }
        DEBUG_OUT("SteerCmd Trigger");
        return cmd_id_;
    }

    /**
     * @brief 打开或关闭某项功能
     *
     * 该函数用于控制某项功能的打开或关闭状态。
     *
     * @param open 控制功能的打开或关闭状态。true 表示打开，false 表示关闭。
     */
    void Open(bool open) {
        msg_cmd_.id = cmd_id_;
        msg_cmd_.open = open;
        if (!open) {
            DEBUG_OUT("Set steer open false");
        }
    }

    /**
     * @brief 设置AGV的指令速度
     *
     * 设置AGV的指令速度，包括线性速度（X和Y方向）和角速度（Z方向），以及转向方向。
     *
     * @param vel 指令速度结构体，包含X和Y方向的线性速度以及Z方向的角速度
     * @param steer_dir 转向方向，具体值参考agv_msgs::msg::DoSteerDir枚举类型
     */
    void SetCmdVel(const Velocity& vel, int steer_dir) {
        msg_cmd_.cmd.linear.x = vel.vel_x_;
        msg_cmd_.cmd.linear.y = vel.vel_y_;
        msg_cmd_.cmd.angular.z = vel.angular_;
        msg_cmd_.steer_dir = steer_dir;
        msg_cmd_.prohibit_limit_flip = (steer_dir != agv_msgs::msg::DoSteerDir::DIR_AUTO);
        DEBUG_WARN_OUT("Steer: SetCmdVel: Vx=" << msg_cmd_.cmd.linear.x
                                                 << ", Vy=" << msg_cmd_.cmd.linear.y 
                                                 << ", angZ=" << msg_cmd_.cmd.angular.z
                                                 << ", msg_cmd_.steer_dir=" << msg_cmd_.steer_dir
                                                 << ", prohibit_limit_flip=" << static_cast<int>(msg_cmd_.prohibit_limit_flip));
    }

    /**
     * @brief 清除命令
     *
     * 将消息命令中的 open、prohibit_limit_flip 和 steer_dir 属性重置为默认值，
     * 并输出调试信息，表明已清除一次转向命令，并清除目标延迟检查标志。
     */
    void Clear() {
        msg_cmd_.open = false;
        msg_cmd_.prohibit_limit_flip = false;
        msg_cmd_.steer_dir = agv_msgs::msg::DoSteerDir::DIR_AUTO;
        DEBUG_OUT("SteerCmd clear once; clear target delay check flag");
    }

public:
    agv_msgs::msg::DoSteerDir msg_cmd_;
    DataTimeStamp<agv_msgs::msg::SteerDirFeedBack> msg_fb_;
    TimerTriggerOnce target_delay_once_; // 延迟检测舵角到位一次
    TimerTriggerOnce timer_readyt_delay_; // 打舵之前强制延迟
private:
    int64_t cmd_id_; // 命令ID
};
